/*!
  @file preprocessor.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _AXIS_NETCAM_PREPROCESSOR_H_
#define _AXIS_NETCAM_PREPROCESSOR_H_

#include <ros/ros.h>
#include <opencv2/core/core.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

namespace axis_netcam 
{

/*! 
 *  @brief  Image preprocessor
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @author Takuma Honda <t_honda@ac.jaxa.jp>
 *  @date   2013
 *  
 *  Undistort and rectify image
 */

class Preprocessor 
{
 public:
  explicit Preprocessor(const ros::NodeHandle &nh) 
      : nh_(nh)
      , private_nh_("~")
      , it_(nh)
  { 
    /* initializing pub/sub, server */
    img_sub_ = it_.subscribeCamera(nh_.resolveName("image_in"), 1, &Preprocessor::imageCB, this);
    img_pub_ = it_.advertiseCamera(nh_.resolveName("image_out"), 1);

    /* initializing optional parameters */
    /*   e.g.) private_nh_.param<type>("param_name", param_var, default_value);  */
  }

  ~Preprocessor() { }

  //! callback funcion for Image and CameraInfo
  void imageCB(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &info_msg);


 private:
  //! check variables before processing
  bool initCompute()
  {
    if (!K_.data || !D_.data) return false;
    return true;
  }

  //! undistort image
  void undistort();

  //! rectify image for stereo processing (optional)
  void rectify();


  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;
  image_transport::ImageTransport it_;
  image_transport::CameraPublisher  img_pub_;
  image_transport::CameraSubscriber img_sub_;

  std::string camera_id_;
  cv::Mat image_;
  cv::Mat undistortimage_;
  cv::Mat K_;    //!< 3x3 intrinsic matrix
  cv::Mat D_;    //!< 5x1 distortion matrix
};

}  // namespace axis_netcam

#endif  // _AXIS_NETCAM_PREPROCESSOR_H_

